// ros头文件
#include "prometheus_gimbal_control/GimbalControl.h"
#include "prometheus_gimbal_control/GimbalState.h"
#include "prometheus_gimbal_control/VisionDiff.h"
#include "prometheus_msgs/ControlCommand.h"
#include "prometheus_msgs/DroneState.h"
#include "printf_utils.h"

#include <ros/ros.h>
#include <iostream>
#include <std_srvs/SetBool.h>
#include <sstream>

float default_pitch;
float kpx_track; //追踪比例系数
float kp_gimbal;

float land_move_scale; //降落移动系数
float land_high;
float static_vel_thresh;
float ignore_error_pitch;
float max_vel, max_yaw_rate;

int max_high = 10; // 丢失目标后, 上升的最大高度
// 目标丢失时速度衰减
float object_lost_vel_weaken;

const int horizontal_max_count = 10;
int horizontal_count = horizontal_max_count;

//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_now;  //发送给position_control.cpp的命令
prometheus_msgs::ControlCommand Command_past; //发送给position_control.cpp的命令

float curr_pos[3];
float curr_vel[3];
std::string curr_mode;
void droneStateCb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
    curr_pos[0] = msg->position[0];
    curr_pos[1] = msg->position[1];
    // curr_pos[2] = msg->position[2]
    curr_pos[2] = msg->rel_alt;

    curr_vel[0] = msg->velocity[0];
    curr_vel[1] = msg->velocity[1];
    curr_vel[2] = msg->velocity[2];

    curr_mode = msg->mode;
}

inline float clamp(float value, float max)
{
    max = std::abs(max);
    return std::fmin(std::fmax(value, -max), max);
}

float pixel_x, pixel_y;
float pic_width;
float pic_high;
int flag_detected = 0; // 是否检测到目标标志
void VisionCb(const prometheus_gimbal_control::VisionDiff::ConstPtr &msg)
{
    flag_detected = msg->detect;
    pixel_x = msg->objectX + msg->objectWidth / 2 - msg->frameWidth / 2;
    pixel_y = msg->objectY + msg->objectHeight / 2 - msg->frameHeight / 2;
    pic_width = msg->frameWidth;
    pic_high = msg->frameHeight;
}

float gimbal_roll, gimbal_pitch, gimbal_yaw;
void GimbalStateCb(const prometheus_gimbal_control::GimbalState::ConstPtr &msg)
{
    // g_gimbal_state = *msg;
    gimbal_roll = msg->imuAngle[0];
    gimbal_pitch = msg->imuAngle[1];
    gimbal_yaw = msg->rotorAngle[2];
}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "circlex_land");
    ros::NodeHandle nh("~");

    // 获取 无人机ENU下的位置
    ros::Subscriber curr_pos_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, droneStateCb);

    // 获取吊舱状态
    ros::Subscriber gimbal_state_sub = nh.subscribe<prometheus_gimbal_control::GimbalState>("/gimbal/state", 10, GimbalStateCb);

    // 获取视觉反馈
    ros::Subscriber vision_sub = nh.subscribe<prometheus_gimbal_control::VisionDiff>("/gimbal/track", 10, VisionCb);

    // 【发布】发送给position_control.cpp的命令
    ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);

    // 手动控制吊舱. 控制初始角度向下
    ros::Publisher gimbal_control_pub = nh.advertise<prometheus_gimbal_control::GimbalControl>("/gimbal/control", 10);

    // 控制吊舱是否接受tracking测器的控制
    ros::ServiceClient gimbal_control_flag_client = nh.serviceClient<std_srvs::SetBool>("/gimbal/stop_track");
    gimbal_control_flag_client.waitForExistence();

    // 频率 [25Hz]
    // 这个频率取决于视觉程序输出的频率，一般不能低于10Hz，不然追踪效果不好
    ros::Rate rate(25);

    // 降落的吊舱初始角度， 朝下
    prometheus_gimbal_control::GimbalControl gimbal_control_;

    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    //追踪算法比例参数
    nh.param<float>("kpx_track", kpx_track, 0.005);
    nh.param<float>("kp_gimbal", kp_gimbal, 0.005);

    //吊舱默认向下的转的角度
    nh.param<float>("default_pitch", default_pitch, -60.);

    // 降落速度
    nh.param<float>("land_move_scale", land_move_scale, 0.2);
    // 自动降落高度
    nh.param<float>("land_high", land_high, 0.2);

    // 目标丢失时速度衰减
    nh.param<float>("object_lost_vel_weaken", object_lost_vel_weaken, 0.5);

    // 忽略的俯仰角误差: 在误差内,判定为无人机已经到达降落板的正上方
    nh.param<float>("ignore_error_pitch", ignore_error_pitch, 5);
    // 静止速度
    nh.param<float>("static_vel_thresh", static_vel_thresh, 0.04);

    nh.param<float>("max_vel", max_vel, 1);

    nh.param<float>("max_yaw_rate", max_yaw_rate, 1);

    constexpr int cv_land_count = 5;
    int cv_land_cnt[cv_land_count]{0, 0, 0, 0, 0};

    enum check_flag
    {
        yaw_pitch_ctl = 1,
        roll_pitch_ctl = 2
    } ctl_type;

    // 降落状态量
    enum State
    {
        Horizontal,
        Vertical,
    };

    int tmp;
    std::cout
        << "Please check the parameter and setting, \n 1 for (yaw + pitch) control \n 2 for mode (roll + pitch) control \n else for quit: " << std::endl;
    try
    {
        std::cin >> tmp;
        switch (tmp)
        {
        case 1:
            ctl_type = yaw_pitch_ctl;
            break;
        case 2:
            // 控制roll轴控制
            static ros::ServiceClient is_roll_client = nh.serviceClient<std_srvs::SetBool>("/circlex_gimbal_roll");
            is_roll_client.waitForExistence();
            std_srvs::SetBool tmp;
            tmp.request.data = true;
            is_roll_client.call(tmp);
            ctl_type = roll_pitch_ctl;
            break;
        }
    }
    catch (const std::runtime_error &e)
    {
        return 0;
    }

    // 吊舱位置初始化
    // 摄像头向下偏转一定角度
    gimbal_control_.rpyMode = prometheus_gimbal_control::GimbalControl::manual;
    gimbal_control_.roll = prometheus_gimbal_control::GimbalControl::angleCtl;
    gimbal_control_.yaw = prometheus_gimbal_control::GimbalControl::angleCtl;
    gimbal_control_.pitch = prometheus_gimbal_control::GimbalControl::angleCtl;
    gimbal_control_.rValue = 0;
    gimbal_control_.pValue = default_pitch;
    gimbal_control_.yValue = 0;
    gimbal_control_pub.publish(gimbal_control_);

    // 丢失时使用的数据
    float lost_x_vel, lost_y_vel, lost_z_vel;
    int land_cnt = 0;
    bool cv_land = false;

    constexpr int max_count_vision_lost = 25 * 2; //视觉丢失计数器，25Hz，就是两秒没有检测目标
    int count_vision_lost;                        //视觉丢失计数器阈值
    count_vision_lost = max_count_vision_lost;

    // 状态初值
    State Now_State = State::Horizontal;
    Command_past.Mode = prometheus_msgs::ControlCommand::Hold;
    Command_now.source = "circlex_landing";
    Command_now.Command_ID = 0;
    Command_past.Reference_State.velocity_ref[0] = 0.;
    Command_past.Reference_State.velocity_ref[1] = 0.;
    Command_past.Reference_State.velocity_ref[2] = 0.;
    Command_past.Reference_State.position_ref[0] = 0.;
    Command_past.Reference_State.position_ref[1] = 0.;
    Command_past.Reference_State.position_ref[2] = 0.;

    while (ros::ok())
    {
        ros::spinOnce();
        // 检查是否处于OFFBOARD模式
        std::stringstream ss;
        if (curr_mode != "OFFBOARD")
            ss << "Waiting for enable OFFBOARD... \n";
        Command_now.Command_ID++;
        Command_now.header.stamp = ros::Time::now();

        float comm[4]{0, 0, 0, 0};
        Command_now.Mode = prometheus_msgs::ControlCommand::Move;
        Command_now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
        Command_now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
        Command_now.Reference_State.Yaw_Rate_Mode = true;

        // 高度低于设定的自动降落高度land_high，或者cv_land标志位为true，就进入Land模式，吊舱home
        if ((curr_pos[2] < land_high || cv_land) && curr_mode == "OFFBOARD")
        {
            Command_now.Mode = prometheus_msgs::ControlCommand::Land;
            gimbal_control_.rpyMode = prometheus_gimbal_control::GimbalControl::home;
            gimbal_control_pub.publish(gimbal_control_);
        }
        // 如果没有发现目标, 进入目标寻回
        else if (flag_detected == 0 && curr_mode == "OFFBOARD")
        {
            int tmp_cv = 0;
            for (int i = 0; i < cv_land_count; i++)
            {
                tmp_cv += cv_land_cnt[i];
                cv_land_cnt[i] = 0;
            }
            // 距离目标过近,导致找不到目标
            if (tmp_cv == cv_land_count)
            {
                cv_land = true;
                PCOUT(0, YELLOW, "5 consecutive detections as X before the target disappears, enter LAND");
            }
            // 目标丢失
            else if (count_vision_lost <= 0) // 进入重新寻回目标模式
            {
                if (curr_pos[2] > max_high) // 上升超过最大高度没有找到目标
                {
                    cv_land = true;
                    PCOUT(0, RED, "mission failed");
                    continue;
                }
                else // 未超过最大高度
                {
                    Command_now.Reference_State.velocity_ref[0] = -lost_x_vel;
                    Command_now.Reference_State.velocity_ref[1] = -lost_y_vel;
                    Command_now.Reference_State.velocity_ref[2] = -lost_z_vel;
                    Command_now.Reference_State.yaw_rate_ref = 0;
                    lost_x_vel *= object_lost_vel_weaken;
                    lost_y_vel *= object_lost_vel_weaken;
                    PCOUT(1, YELLOW, "try to find the target");
                }
            }
            // 用于在无人机在频繁的交替丢失目标时稳定飞行
            else
            {
                // 记录目标丢失时数据
                if (count_vision_lost == max_count_vision_lost)
                {
                    lost_x_vel = Command_past.Reference_State.velocity_ref[0];
                    lost_y_vel = Command_past.Reference_State.velocity_ref[1];
                    lost_z_vel = Command_past.Reference_State.velocity_ref[2];
                    lost_z_vel = -std::fmax(std::abs(lost_z_vel), 0.3);
                }
                Command_now.Reference_State.velocity_ref[0] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[0];
                Command_now.Reference_State.velocity_ref[1] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[1];
                Command_now.Reference_State.velocity_ref[2] = object_lost_vel_weaken * Command_past.Reference_State.velocity_ref[2];
                Command_now.Reference_State.yaw_rate_ref = 0;
                PCOUT(1, YELLOW, "object lost, use past command");
                count_vision_lost--;
            }
        }
        // 发现目标，准备降落
        else
        {
            // 重置丢失次数
            count_vision_lost = max_count_vision_lost;
            switch (ctl_type)
            {
                case yaw_pitch_ctl:
                    switch (Now_State)
                    {
                        case State::Horizontal:
                        {
                            // 如果无人机在接近降落板的正上方时, 锁定吊舱偏航角, 锁定无人机偏航角对齐吊舱
                            if (std::abs(90 - gimbal_pitch) < ignore_error_pitch)
                            {
                                // 等待一会再进入下降
                                if (horizontal_count > 0)
                                {
                                    horizontal_count--;
                                }
                                // 进入垂直下降
                                else
                                {
                                    // 1. 关闭吊舱跟随
                                    std_srvs::SetBool tmp;
                                    tmp.request.data = true;
                                    gimbal_control_flag_client.call(tmp);

                                    // 2. 锁死patch -90, 锁死yaw
                                    gimbal_control_.rpyMode = prometheus_gimbal_control::GimbalControl::manual;
                                    gimbal_control_.roll = prometheus_gimbal_control::GimbalControl::angleCtl;
                                    gimbal_control_.pitch = prometheus_gimbal_control::GimbalControl::angleCtl;
                                    gimbal_control_.yaw = prometheus_gimbal_control::GimbalControl::angleCtl;
                                    gimbal_control_.rValue = 0;
                                    gimbal_control_.pValue = 90;
                                    gimbal_control_.yValue = 0;
                                    gimbal_control_pub.publish(gimbal_control_);
                                    PCOUT(0, YELLOW, "yaw lock, patch lock");
                                    Now_State = State::Vertical;
                                }
                            }
                            else // 无人机还没到降落板正上方，控yaw, xy速度
                            {
                                // 直接根据gimbal_pitch角度调整前进的速度, 并且在航向角误差越大时，前进到速度越慢
                                comm[0] = kpx_track * (90. - gimbal_pitch) / std::fmax(std::abs(gimbal_yaw) - 5, 1) * 2;
                                comm[1] = 0;
                                comm[2] = 0;
                                comm[3] = -gimbal_yaw * kp_gimbal;
                                ss << "\n >> horizontal move \n ";
                            }
                            break;
                        }
                        case State::Vertical:
                        {
                            // float offset = std::max(curr_pos[2] * 0.10, 0.10);
                            float offset = land_move_scale * curr_pos[2];

                            // 垂直观测范围:39.8°(近焦) 到 4.2°(远焦)。
                            // tan(53.2/2)=0.50
                            comm[0] = -((pixel_y - 50) / (pic_high * 0.5)) * std::tan(M_PI * (39.8 / 2) / 180) * offset;
                            // 水平观测范围:53.2°(近焦) 到 5.65°(远焦)。
                            // tan(53.2/2)=0.50
                            comm[1] = -(pixel_x / (pic_width * 0.5)) * std::tan(M_PI * (53.2 / 2) / 180) * offset;
                            // ((480 + 640)/2)/10 = 56
                            comm[2] = -offset / std::fmax(std::sqrt(std::pow(pixel_y, 2) + std::pow(pixel_x, 2)) / 20, 1);
                            comm[3] = 0.;
                            ss << "\n >> height move << \n"
                            << " pixel_error_x: " << pixel_x
                            << " pixel_error_y: " << pixel_y
                            << "\n";
                            break;
                        }
                    }
                    break;
                case roll_pitch_ctl:
                    // 靠近时, 不控航向角
                    if (std::abs(90 - gimbal_pitch) < ignore_error_pitch)
                    {
                        comm[0] = kpx_track * (90 - gimbal_pitch + 2) * curr_pos[2] * 0.5 * 0.5;
                        comm[1] = kpx_track * gimbal_roll * curr_pos[2] * 0.5 * 0.5;
                        comm[2] = -land_move_scale * curr_pos[2] / (((90. - gimbal_pitch) + gimbal_roll)/3);
                        comm[3] = 0;
                        // 吊舱能达到的最大角度为90度
                        if (90 - gimbal_pitch < 1)
                        {
                            comm[0] = -(std::fmin(pixel_y - 50, 0) / (pic_high * 0.5)) * land_move_scale * curr_pos[2];
                        }
                    }
                    else
                    {
                        comm[0] = kpx_track * (90 - gimbal_pitch) * curr_pos[2] / std::fmax(std::abs(gimbal_roll) / 5, 1) * 0.5;
                        comm[1] = 0;
                        comm[2] = -kpx_track * std::fmin(1 / (90 - gimbal_pitch), 0.1) * curr_pos[2];
                        comm[3] = kp_gimbal * gimbal_roll * 2;
                    }
                    break;
            }
            // 视觉降落判断
            // land_cnt 的值在 0 ~ (cv_land_count-1)==4 之间循环
            // 连续5个周期 flag_detected == 2 则 cv_land_cnt[]==1
            land_cnt = ++land_cnt % cv_land_count;
            cv_land_cnt[land_cnt] = flag_detected == 2 ? 1 : 0;

            // 检测到X且当前速度被视为静止状态, 进入LAND
            if (flag_detected == 2 && std::abs(comm[1]) + std::abs(comm[2]) < static_vel_thresh)
            {
                cv_land = true;
                comm[0] = 0;
                comm[1] = 0;
                comm[2] = 0;
                comm[3] = 0;
            }
        }

        // 发布速度和航向命令给底层
        for (int i = 0; i < 3; i++)
        {
            Command_now.Reference_State.velocity_ref[i] = clamp(comm[i], max_vel);
        }
        Command_now.Reference_State.yaw_rate_ref = (clamp(comm[3], max_yaw_rate) / 180.0) * M_PI;
        command_pub.publish(Command_now);

        ss << "max_veloity: " << max_vel << " [m/s] \n"
           << "x_velocity: " << comm[0] << " [m/s] "
           << "y_velocity: " << comm[1] << " [m/s] "
           << "z_velocity: " << comm[2] << " [m/s]\n"
           << "gimbal_roll: " << gimbal_roll << " [ang/s] "
           << "gimbal_pitch: " << gimbal_pitch << " [ang/s] "
           << "gimbal_yaw: " << gimbal_yaw << " [ang/s] \n"
           << "drone height: " << curr_pos[2] << " m \n"
           << "max yaw rate: " << max_yaw_rate << " [ang/s] \n"
           << "yaw rate: " << comm[3] << " [ang/s] \n"
           << "target distance estimate: " << (gimbal_pitch < 88 ? curr_pos[2] * std::tan(gimbal_pitch * M_PI / 180) :  curr_pos[2]) << " [m]\n";
        PCOUT(1, TAIL, ss.str());

        // 控制降落
        while (Command_now.Mode == prometheus_msgs::ControlCommand::Land)
        {
            command_pub.publish(Command_now);
            rate.sleep();
            ros::spinOnce();
            if (curr_mode != "AUTO.LAND")
                continue;
            cout << "LAND FINISH, PRESS ANY KEY TO EXIT" << endl;
            // 防止程序直接退出
            cin >> tmp;
            return 0;
        }

        Command_past = Command_now;

        rate.sleep();
    }
    return 0;
}
